/*
 * Left or Right trigger (but not both at the same time) Shoot frisbes
 * Button X - Turn Shooter Wheels on
 * Button Y - Turn Sooter wheels off
 * Button A pressed - Allow frisbes to slide down chute int shooter area
 * Button A released - Does not allow frisbes to slide down chute int shooter area
 * Right Bumper (press/release) - Activate Current Drive - deactivate voltage drive
 * Left Bumper (press/release) - Dectivate Current Drive - activate voltage drive
 * 
 */
//#define PROTOTYPE
#define COMPETITION
//#define ROBOT_DRIVE_2 
#define ROBOT_DRIVE_4
//#define ARCADEDRIVE
#define SHOOTER
#define TANKDRIVE
//#define CURRENTMODE
//#define VIMODE
#define VIMODE_B  // Manual switching between current and voltage mode
//#define VIMODE_C // Automatic switching between current and voltage mode Depending on trip and reset points - Independent switching
//#define VIMODE_C2 // Automatic switching between current and voltage mode Depending on trip and reset points - syncronized switching

#define CURRENT_TRIP_POINT 35.0
#define CURRENT_RESET_POINT 30.0
// TODO: Tune PID
#define JAGP 7.0
#define JAGI 1.0
#define JAGD 1.0
//#define CLIMBER_DEFINED
#include "WPILib.h"
#include "RobotDrive2012.hpp"
#include "Shooter.hpp"
#include "joysticks.hpp"
#include "Climber.hpp"
#include "CANJaguar1405.hpp"
#define RAMPRATE 120.0  //   volts /sec
#define COMPRESSOR_RELAY_CHANNEL 2
#define COMPRESSOR_SWITCH_CHANNEL 2
#define COMPRESSOR_ON 0
#define LEFT_DRIVE_CHANNEL 2
//#define RIGHT_DRIVE_CHANNEL 4 //For Robot
#define RIGHT_DRIVE_CHANNEL 4 // For Prototype system
#define AUTO_TIME 10
#define AUTO_DROP_TIME 2
#define AUTO_SHOOT_MOTOR_1 .9 // TODO Set correct Values from 001 computer
#define AUTO_SHOOT_MOTOR_2 .8
#define AUTO_SHOT_COUNT 3
#define FAST_SHOOT_MOTOR_1 .9 // TODO Set correct Values from 001 computer
#define FAST_SHOOT_MOTOR_2 .8
#define SLOW_SHOOT_MOTOR_1 .9
#define SLOW_SHOOT_MOTOR_2 .8
#define AUTO_MOTOR_RAMP_TIME 5
#define CAL_BLINK .5
#define MAXCLIMBSPEED -0.5
#define BATTERY1 3
#define BATTERY2 4
#define CRITICAL_BATTERY_VOLTAGE 12
#define STICK_FACTOR 1.0 
//For 2013 robot
//#define STICK_FACTOR -.75 // For 2013 prototype
#define JAGCALZEROTIME 0.5
#define JAGCALRUNTIME 2.0
#define JAGCALSPEED 40.0



/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */ 
class RobotDemo : public SimpleRobot
{
#ifdef ROBOT_DRIVE_2
	RobotDrive myRobot; // robot drive system
	CANJaguar1405 Jag1, Jag2;
#endif
#ifdef ROBOT_DRIVE_4
//	RobotDrive myRobot;
	CANJaguar1405 Jag1, Jag2, Jag3, Jag4; 
//	CANJaguar Jag1, Jag2, Jag3, Jag4; 
		
#endif
	Compressor myCompressor;
	DigitalOutput BatteryStatus1,BatteryStatus2;
	AnalogChannel BatteryVoltage;
	XBox360 stick; // only joystick
#ifdef SHOOTER
	Shooter MyShooter;
#endif
	Timer CalTimer;

#ifdef CLIMBER_DEFINED
	Climber MyClimber;
#endif
public:
	Timer AutoTimer, JagCalTimer;
protected:
	SimpleDeadband DeadBand;
	float LeftStick, RightStick,ClimbSpeed;
	bool IsShootSpeedFast,IsShooterOn,IsClimb,IsCompressorOn,IsStartButton;
	double ShootMotor1,ShootMotor2;
	bool IsCurentModeChange;
	bool IsCurrentMode;
	bool IsCurentModeChange1;
	bool IsCurrentMode1;
	bool IsCurentModeChange2;
	bool IsCurrentMode2;
	bool IsCurentModeChange3;
	bool IsCurrentMode3;
	bool IsCurentModeChange4;
	bool IsCurrentMode4;
	bool IsLogInitialized;
	float LeftStick1,LeftStick2,RightStick3,RightStick4;
	bool IsInitialized;
public:
	RobotDemo(void):
#ifdef ROBOT_DRIVE_2
		// myRobot(LEFT_DRIVE_CHANNEL,RIGHT_DRIVE_CHANNEL),	// these must be initialized in the same order
#endif 
#ifdef ROBOT_DRIVE_4
		//myRobot(Jag1, Jag2, Jag3, Jag4), 
#ifdef PROTOTYPE
		Jag1(2), Jag2(4), Jag3(3), Jag4(7), 
#endif	
#ifdef COMPETITION
#ifdef ROBOT_DRIVE_2
		Jag1(2), Jag2(4),
#endif
#ifdef ROBOT_DRIVE_4
		Jag1(2), Jag2(3), Jag3(4), Jag4(7),
#endif
#endif
#endif
		myCompressor(COMPRESSOR_SWITCH_CHANNEL,COMPRESSOR_RELAY_CHANNEL),
		BatteryStatus1(BATTERY1),BatteryStatus2(BATTERY2),
		BatteryVoltage(7),
		stick(1)		// as they are declared above.
	{ 
		Jag1.SetVoltageRampRate(RAMPRATE);
		Jag2.SetVoltageRampRate(RAMPRATE);
		Jag1.ConfigFaultTime(0.5);
		Jag2.ConfigFaultTime(0.5);
#ifdef VIMODE
		Jag1.EnableControl();
		Jag2.EnableControl();
		Jag1.ChangeControlMode(CANJaguar1405::kPercentVI);
		Jag2.ChangeControlMode(CANJaguar1405::kPercentVI);	
		Jag1.SetPID(JAGP,JAGI,JAGD);
		Jag2.SetPID(JAGP,JAGI,JAGD);

#endif
#ifdef CURRENTMODE
		Jag1.EnableControl();
		Jag2.EnableControl();
		Jag1.ChangeControlMode(CANJaguar1405::kCurrent);
		Jag2.ChangeControlMode(CANJaguar1405::kCurrent);	
		Jag1.SetPID(JAGP,JAGI,JAGD);
		Jag2.SetPID(JAGP,JAGI,JAGD);
#endif
#ifdef ROBOT_DRIVE_4 
		Jag3.SetVoltageRampRate(RAMPRATE);
		Jag3.ConfigFaultTime(0.5);
		Jag4.SetVoltageRampRate(RAMPRATE);
		Jag4.ConfigFaultTime(0.5);
#ifdef VIMODE
//		Jag3.EnableControl();
//		Jag4.EnableControl();
		Jag3.ChangeControlMode(CANJaguar1405::kPercentVI);
		Jag4.ChangeControlMode(CANJaguar1405::kPercentVI);	
//		Jag3.SetPID(JAGP,JAGI,JAGD);
//		Jag4.SetPID(JAGP,JAGI,JAGD);

#endif
#ifdef CURRENTMODE
		Jag3.EnableControl();
		Jag4.EnableControl();
		Jag3.ChangeControlMode(CANJaguar1405::kCurrent);
		Jag4.ChangeControlMode(CANJaguar1405::kCurrent);
		Jag3.SetPID(JAGP,JAGI,JAGD);
		Jag4.SetPID(JAGP,JAGI,JAGD);
#endif
#endif
		printf("%s %s %s\n",__FILE__,__DATE__,__TIME__);
		myCompressor.Start();
		
		DeadBand.Setband(0.08);
		IsShootSpeedFast=true;
		IsShooterOn=false;
		IsClimb=false;
		IsCurentModeChange=false;
		IsCurrentMode = false;
		IsCompressorOn=false;
		IsStartButton=false;
		IsLogInitialized=false;
		ClimbSpeed=0;
		ShootMotor1=AUTO_SHOOT_MOTOR_1;
		ShootMotor2=AUTO_SHOOT_MOTOR_2;
//		myRobot.SetExpiration(10);

//		SmartDashboard::init();
//		SmartDashboard::PutBoolean("Log Check",false);
/*		SmartDashboard::PutBoolean("CalCheck",false);
		SmartDashboard::PutBoolean("Calibrate",false);
		SmartDashboard::PutBoolean("Status",false);
		SmartDashboard::PutBoolean("StatusCheck",false);
		SmartDashboard::PutBoolean("Start Jag Test",false);
		SmartDashboard::PutNumber("P", JAGP);
		SmartDashboard::PutNumber("I", JAGI);
		SmartDashboard::PutNumber("D", JAGD);
		SmartDashboard::PutNumber("Zero Time", JAGCALZEROTIME);
		SmartDashboard::PutNumber("Run Time", JAGCALRUNTIME);
		SmartDashboard::PutNumber("Jag Calibration Run Speed", JAGCALSPEED);
		SmartDashboard::PutBoolean("Calibrate Jags",false);
*/
#ifdef SHOOTER
//		SmartDashboard::PutNumber("Shot Count",MyShooter.ShotCount());
#endif


		

	}

	/**
	 * Drive left & right motors for 2 seconds then stop
	 */
	void Autonomous(void)
	{
#ifdef SHOOTER
//		SmartDashboard::PutNumber("Shot Count",MyShooter.ShotCount());		
	
//		myRobot.SetSafetyEnabled(false);
		Jag1.SetSafetyEnabled(false);
		Jag2.SetSafetyEnabled(false);
		Jag3.SetSafetyEnabled(false);
		Jag4.SetSafetyEnabled(false);
		AutoTimer.Reset();
		AutoTimer.Start();
		MyShooter.SetSpeed(AUTO_SHOOT_MOTOR_1,AUTO_SHOOT_MOTOR_2);
		AutoTimer.Reset();
		AutoTimer.Start();
		while (!AutoTimer.HasPeriodPassed(AUTO_MOTOR_RAMP_TIME))
		{
			MyShooter.Update();
#ifdef ARCADEDRIVE
	//		myRobot.ARCADEDRIVE(0.0, 0.0);
			Jag1.Set(0.0);
			Jag2.Set(0.0);
			Jag3.Set(0.0);
			Jag4.Set(0.0);
#endif
#ifdef TANKDRIVE
//			myRobot.TankDrive(0.0, 0.0);
					Jag1.Set(0.0);
					Jag2.Set(0.0);
					Jag3.Set(0.0);
					Jag4.Set(0.0);
#endif
			Wait (.005);
		}
		MyShooter.ResetShotCount();
		while (!MyShooter.HaveShot(AUTO_SHOT_COUNT))
		{
		MyShooter.Launch();
		MyShooter.Update();
//		SmartDashboard::PutNumber("Shot Count",MyShooter.ShotCount());
#ifdef ARCADEDRIVE
//		myRobot.ArcadeDrive(0.0, 0.0);
		Jag1.Set(0.0);
		Jag2.Set(0.0);
		Jag3.Set(0.0);
		Jag4.Set(0.0);
#endif
#ifdef TANKDRIVE
//		myRobot.TankDrive(0.0, 0.0);
		Jag1.Set(0.0);
		Jag2.Set(0.0);
		Jag3.Set(0.0);
		Jag4.Set(0.0);
#endif
		Wait (.005);
		}
		AutoTimer.Reset();
		AutoTimer.Start();
		while (!AutoTimer.HasPeriodPassed(AUTO_DROP_TIME))
		{
			MyShooter.Drop(true);
			MyShooter.Update();
#ifdef ARCADEDRIVE
//			myRobot.ArcadeDrive(0.0, 0.0);

			Jag1.Set(0.0);
			Jag2.Set(0.0);
			Jag3.Set(0.0);
			Jag4.Set(0.0);
#endif
#ifdef TANKDRIVE
//			myRobot.TankDrive(0.0, 0.0);
			Jag1.Set(0.0);
			Jag2.Set(0.0);
			Jag3.Set(0.0);
			Jag4.Set(0.0);
#endif
			Wait (.005);
		}
		MyShooter.ResetShotCount();
		while (!MyShooter.HaveShot(AUTO_SHOT_COUNT))
		{
		MyShooter.Launch();
		MyShooter.Update();
//		SmartDashboard::PutNumber("Shot Count",MyShooter.ShotCount());
#ifdef ARCADEDRIVE
//		myRobot.ArcadeDrive(0.0 0.0);
		Jag1.Set(0.0);
		Jag2.Set(0.0);
		Jag3.Set(0.0);
		Jag4.Set(0.0);
#endif
#ifdef TANKDRIVE
//		myRobot.TankDrive(0.0, 0.0);
		Jag1.Set(0.0);
		Jag2.Set(0.0);
		Jag3.Set(0.0);
		Jag4.Set(0.0);
#endif
		Wait (.005);
		}

		AutoTimer.Stop();
		MyShooter.SetSpeed(0,0);
		MyShooter.Drop(false);
		Wait (.005);
		MyShooter.Update();
#endif
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
//		myRobot.SetSafetyEnabled(false);
		Jag1.SetSafetyEnabled(false);
		Jag2.SetSafetyEnabled(false);
		Jag3.SetSafetyEnabled(false);
		Jag4.SetSafetyEnabled(false);
#ifdef SHOOTER
		MyShooter.SetSpeed(0,0);
#endif
#ifdef CLIMBER_DEFINED
		MyClimber.SetSpeed(0);
#endif
		IsStartButton=true;
		while (IsOperatorControl())
		{
//			if(SmartDashboard::GetBoolean("CalCheck"))Calibrate();
			
			if(stick.GetButtonStart())IsStartButton=true;
			if(stick.GetButtonBack())IsStartButton=false;
			Jag1.ConfigFaultTime(0.5);
			Jag2.ConfigFaultTime(0.5);
			Jag3.ConfigFaultTime(0.5);
			Jag4.ConfigFaultTime(0.5);
		if(IsStartButton)
			{
				myCompressor.Start();
			}
			else
			{
				myCompressor.Stop();
			}
			
#ifdef SHOOTER
		MyShooter.Drop(stick.GetButtonA());
/*		if(stick.GetButtonA())IsClimb=IsShootSpeedFast=false;
			if(stick.GetButtonB())IsClimb=IsShootSpeedFast=true;

			if(IsShootSpeedFast)
			{
				ShootMotor1=FAST_SHOOT_MOTOR_1;
				ShootMotor2=FAST_SHOOT_MOTOR_2;
			}
			else
			{
				ShootMotor1=SLOW_SHOOT_MOTOR_1;
				ShootMotor2=SLOW_SHOOT_MOTOR_2;				
			}
*/				
			if(stick.GetButtonX()){MyShooter.SetSpeed(ShootMotor1,ShootMotor2);IsShooterOn=true;}
			if(stick.GetButtonY()){MyShooter.SetSpeed(0);IsShooterOn=false;}
			if((stick.GetTrigger()!= 0)&&IsShooterOn)MyShooter.Launch();
			MyShooter.Update();

#endif

			
#ifdef CLIMBER_DEFINED 
			if(stick.GetButtonA())IsClimb=true;
			if(stick.GetButtonB())IsClimb=false;
			if(IsClimb)
			{
				
				// MyClimber.ApplyClamp();
			}
			else
			{
			//	MyClimber.ReleaseClamp();
			}
			MyClimber.SetSpeed(ClimbSpeed);
			MyClimber.Update();
#endif
#ifdef ARCADEDRIVE
		LeftStick=stick.GetLeftStickX();
		RightStick=stick.GetLeftStickY();
#endif
#ifdef TANKDRIVE
		LeftStick=stick.GetLeftStickY();
		RightStick=stick.GetRightStickY();

#endif
		ClimbSpeed=-stick.GetRightStickY();
		if(ClimbSpeed<0)ClimbSpeed=0;
		ClimbSpeed=ClimbSpeed*MAXCLIMBSPEED;
		DeadBand.ModifyValue(LeftStick);
		DeadBand.ModifyValue(RightStick);
		DeadBand.ModifyValue(ClimbSpeed);
		LeftStick=LeftStick*STICK_FACTOR;
		RightStick=RightStick*STICK_FACTOR;
		
// *************  Square stick routine
		// square the inputs (while preserving the sign) to increase fine control while permitting full power
		if (LeftStick >= 0.0)
		{
			LeftStick = (LeftStick * LeftStick);
		}
		else
		{
			LeftStick = -(LeftStick * LeftStick);
		}
		if (RightStick >= 0.0)
		{
			RightStick = (RightStick * RightStick);
		}
		else
		{
			RightStick = -(RightStick * RightStick);
		}
	

//*****************************************************
		
#ifdef  VIMODE_B
		if(stick.GetButtonLeftBumper()){IsCurentModeChange=true;IsCurrentMode=false;}	
		if(stick.GetButtonRightBumper()) {IsCurentModeChange=true;IsCurrentMode=true;}
#endif
// TODO: validCheck to see if VIMODE_C2 mode check is correct
		
#ifdef VIMODE_C
		
		if(LeftStick>0)
		{
		if((Jag1.GetOutputCurrent()<= CURRENT_RESET_POINT)&& IsCurrentMode1) {IsCurentModeChange1=true;IsCurrentMode1=false;}	
		if((Jag1.GetOutputCurrent()>= CURRENT_TRIP_POINT)&& !IsCurrentMode1) {IsCurentModeChange1=true;IsCurrentMode1=true;}
		}
		if(LeftStick<0)
		{
		if((Jag1.GetOutputCurrent()>= -CURRENT_RESET_POINT)&& IsCurrentMode1){IsCurentModeChange1=true;IsCurrentMode1=false;}	
		if((Jag1.GetOutputCurrent()<= -CURRENT_TRIP_POINT)&& !IsCurrentMode1) {IsCurentModeChange1=true;IsCurrentMode1=true;}
		}
		
		if(LeftStick>0)
		{
		if((Jag2.GetOutputCurrent()<= CURRENT_RESET_POINT)&& IsCurrentMode2){IsCurentModeChange2=true;IsCurrentMode2=false;}	
		if((Jag2.GetOutputCurrent()>= CURRENT_TRIP_POINT)&& !IsCurrentMode2) {IsCurentModeChange2=true;IsCurrentMode2=true;}
		}
		if(LeftStick<0)
		{
		if((Jag2.GetOutputCurrent()>= -CURRENT_RESET_POINT)&& IsCurrentMode2){IsCurentModeChange2=true;IsCurrentMode2=false;}	
		if((Jag2.GetOutputCurrent()<= -CURRENT_TRIP_POINT)&& !IsCurrentMode2) {IsCurentModeChange2=true;IsCurrentMode2=true;}
		}
		
		if(LeftStick>0)
		{
		if((Jag3.GetOutputCurrent()<= CURRENT_RESET_POINT)&& IsCurrentMode3){IsCurentModeChange3=true;IsCurrentMode3=false;}	
		if((Jag3.GetOutputCurrent()>= CURRENT_TRIP_POINT)&& !IsCurrentMode3) {IsCurentModeChange3=true;IsCurrentMode3=true;}
		}
		if(LeftStick<0)
		{
		if((Jag3.GetOutputCurrent()>= -CURRENT_RESET_POINT)&& IsCurrentMode3){IsCurentModeChange3=true;IsCurrentMode3=false;}	
		if((Jag3.GetOutputCurrent()<= -CURRENT_TRIP_POINT)&& !IsCurrentMode3) {IsCurentModeChange3=true;IsCurrentMode3=true;}
		}
		
		if(LeftStick>0)
		{
		if((Jag4.GetOutputCurrent()<= CURRENT_RESET_POINT)&& IsCurrentMode4){IsCurentModeChange4=true;IsCurrentMode4=false;}	
		if((Jag4.GetOutputCurrent()>= CURRENT_TRIP_POINT)&& !IsCurrentMode4) {IsCurentModeChange4=true;IsCurrentMode4=true;}
		}
		if(LeftStick<0)
		{
		if((Jag4.GetOutputCurrent()>= -CURRENT_RESET_POINT)&& IsCurrentMode4){IsCurentModeChange4=true;IsCurrentMode4=false;}	
		if((Jag4.GetOutputCurrent()<= -CURRENT_TRIP_POINT)&& !IsCurrentMode4) {IsCurentModeChange4=true;IsCurrentMode4=true;}
		}
		
		
#ifndef VIMODE_C2		
		if(IsCurentModeChange1&& !IsCurrentMode1)
		{
			Jag1.ChangeControlMode(CANJaguar1405::kPercentVbus);
			IsCurentModeChange1=false;	
		}
		if(IsCurentModeChange1&& IsCurrentMode1)
		{
			Jag1.EnableControl();
			Jag1.ChangeControlMode(CANJaguar1405::kCurrent);
			Jag1.SetPID(JAGP,JAGI,JAGD);
			IsCurentModeChange1=false;	
		}
		
		if(IsCurentModeChange2&& !IsCurrentMode2)
		{
			Jag2.ChangeControlMode(CANJaguar1405::kPercentVbus);
			IsCurentModeChange2=false;	
		}
		if(IsCurentModeChange2&& IsCurrentMode2)
		{
			Jag2.EnableControl();
			Jag2.ChangeControlMode(CANJaguar1405::kCurrent);
			Jag2.SetPID(JAGP,JAGI,JAGD);
			IsCurentModeChange1=false;	
		}
		
		if(IsCurentModeChange3&& !IsCurrentMode3)
		{
			Jag3.ChangeControlMode(CANJaguar1405::kPercentVbus);
			IsCurentModeChange3=false;	
		}
		if(IsCurentModeChange3&& IsCurrentMode3)
		{
			Jag3.EnableControl();
			Jag3.ChangeControlMode(CANJaguar1405::kCurrent);
			Jag3.SetPID(JAGP,JAGI,JAGD);
			IsCurentModeChange3=false;	
		}
		
		
		if(IsCurentModeChange4&& !IsCurrentMode4)
		{
			Jag4.ChangeControlMode(CANJaguar1405::kPercentVbus);
			IsCurentModeChange4=false;	
		}
		if(IsCurentModeChange4&& IsCurrentMode4)
		{
			Jag4.EnableControl();
			Jag4.ChangeControlMode(CANJaguar1405::kCurrent);
			Jag4.SetPID(JAGP,JAGI,JAGD);
			IsCurentModeChange4=false;	
		}
#endif
		
#ifdef 	VIMODE_C2
		IsCurrentMode1=IsCurrentMode1|IsCurrentMode2||IsCurrentMode3||IsCurrentMode4;
		IsCurentModeChange1=IsCurentModeChange1||IsCurentModeChange2||IsCurentModeChange3||IsCurentModeChange4;
		if((IsCurentModeChange1)&& !IsCurrentMode1)
		{
			Jag1.ChangeControlMode(CANJaguar1405::kPercentVbus);
			Jag2.ChangeControlMode(CANJaguar1405::kPercentVbus);
			Jag3.ChangeControlMode(CANJaguar1405::kPercentVbus);
			Jag4.ChangeControlMode(CANJaguar1405::kPercentVbus);
			IsCurentModeChange1=IsCurentModeChange2=IsCurentModeChange3=IsCurentModeChange4=false;	
		}
		if(IsCurentModeChange1&& IsCurrentMode1)
		{
			Jag1.EnableControl();
			Jag1.ChangeControlMode(CANJaguar1405::kCurrent);
			Jag1.SetPID(JAGP,JAGI,JAGD);
			Jag2.EnableControl();
			Jag2.ChangeControlMode(CANJaguar1405::kCurrent);
			Jag2.SetPID(JAGP,JAGI,JAGD);
			Jag3.EnableControl();
			Jag3.ChangeControlMode(CANJaguar1405::kCurrent);
			Jag3.SetPID(JAGP,JAGI,JAGD);
			Jag4.EnableControl();
			Jag4.ChangeControlMode(CANJaguar1405::kCurrent);
			Jag4.SetPID(JAGP,JAGI,JAGD);
			IsCurentModeChange1=IsCurentModeChange2=IsCurentModeChange3=IsCurentModeChange4=false;	
		}

#endif

		
		LeftStick1=LeftStick;
		if(IsCurrentMode1)LeftStick1=LeftStick1*40;
		LeftStick2=LeftStick;
		if(IsCurrentMode2)LeftStick1=LeftStick2*40;
		RightStick3=RightStick;
		if(IsCurrentMode3)RightStick3=RightStick3*40;
		RightStick4=RightStick;
		if(IsCurrentMode4)RightStick3=RightStick4*40;
	
		
#endif
#ifdef	VIMODE_B
		if(IsCurentModeChange&& !IsCurrentMode)
		{
			// TODO: Set speed to zero before changing?
			Jag1.DisableControl();
			Jag2.DisableControl();
			Jag3.DisableControl();
			Jag4.DisableControl();
			Jag1.ChangeControlMode(CANJaguar1405::kPercentVbus);
			Jag2.ChangeControlMode(CANJaguar1405::kPercentVbus);	
			Jag3.ChangeControlMode(CANJaguar1405::kPercentVbus);
			Jag4.ChangeControlMode(CANJaguar1405::kPercentVbus);
			Wait(.1);
			IsCurentModeChange=false;
			
		}
		if(IsCurentModeChange&& IsCurrentMode)
		{
			// TODO: Set speed to zero before changing?
			Jag1.EnableControl();
			Jag2.EnableControl();
			Jag3.EnableControl();
			Jag4.EnableControl();
			Jag1.ChangeControlMode(CANJaguar1405::kCurrent);
			Jag2.ChangeControlMode(CANJaguar1405::kCurrent);	
			Jag3.ChangeControlMode(CANJaguar1405::kCurrent);
			Jag4.ChangeControlMode(CANJaguar1405::kCurrent);
			Jag1.SetPID(JAGP,JAGI,JAGD);
			Jag2.SetPID(JAGP,JAGI,JAGD);
			Jag3.SetPID(JAGP,JAGI,JAGD);
			Jag4.SetPID(JAGP,JAGI,JAGD);
			Wait(.1);
			IsCurentModeChange=false;
			
		}

#endif

#ifdef ARCADEDRIVE	
//			myRobot.ArcadeDrive(LeftStick,-RightStick); // drive with arcade style (use right stick)
#endif	
#ifdef TANKDRIVE
#ifdef VIMODE_B
			if(IsCurrentMode){LeftStick=LeftStick*40;RightStick=RightStick*40;}
			Jag1.Set(LeftStick);
			Jag2.Set(LeftStick);
			Jag3.Set(-RightStick);
			Jag4.Set(-RightStick);
//			myRobot.TankDrive(LeftStick, RightStick);
#endif
#ifdef VIMODE_C
			Jag1.Set(LeftStick1);
			Jag2.Set(LeftStick2);
			Jag3.Set(-RightStick3);
			Jag4.Set(-RightStick4);
//			myRobot.TankDrive(LeftStick, RightStick);
#endif
#endif
//			if(SmartDashboard::GetBoolean("Log Check"))Log();
			Wait(0.005);				// wait for a motor update time
		}
	}
	
	/**
	 * Runs during test mode
	 */
	void Test() {

	}
	
	void Disabled ()
	{
		myCompressor.Start();

		while(IsDisabled())
		{
			if(BatteryVoltage.GetVoltage()<=CRITICAL_BATTERY_VOLTAGE)
			{
				BatteryStatus1.Set(false);
				BatteryStatus2.Set(true);
			}
			else
			{
				BatteryStatus1.Set(true);
				BatteryStatus2.Set(false);
			}
			Wait(.1);
		}
	}
	void Status()
	{
			// TODO: Change Status for CANJaguae1405
		/*
#ifdef ROBOTDRIVE
	//	myRobot.Status();
#endif */
	}
	void Calibrate() // TODO: Modify calibrate for change in myRobot with 2 motor drive
	{
/*
		bool BlinkFlg=false, Blinker=false;
		while(SmartDashboard::GetBoolean("CalCheck"))
		{
#ifdef ARCADEDRIVE
//			myRobot.ARCADEDRIVE(0.0, 0.0); // drive with arcade style (use right stick)
#endif
#ifdef TANKDRIVE
			
//			myRobot.TankDrive(0.0, 0.0);
#endif

					if (SmartDashboard::GetBoolean("CalCheck"))
					{
						SmartDashboard::PutBoolean("Calibrate",true);
#ifdef CLIMBER_DEFINED
						MyClimber.Calibrate();
#endif
#ifdef SHOOTER
						MyShooter.Calibrate();
#endif
#ifdef CURRENTMODE
						while(SmartDashboard::GetBoolean("Calibrate Jags"))
						{
							double MyP, MyI, MyD;
							MyP = SmartDashboard::GetNumber("P");
							MyI = SmartDashboard::GetNumber("I");
							MyD = SmartDashboard::GetNumber("D");
							Jag1.SetPID(MyP, MyI, MyD);
							Jag2.SetPID(MyP, MyI, MyD);
							Jag3.SetPID(MyP, MyI, MyD);
							Jag4.SetPID(MyP, MyI, MyD);
							Jag1.InitSMD();
							Jag2.InitSMD();
							Jag3.InitSMD();
							Jag4.InitSMD();
							float mySpeed = SmartDashboard::GetNumber("Jag Calibration Run Speed");
							Jag1.Set((float) 0.0);
							Jag2.Set((float) 0.0);
							Jag3.Set((float) 0.0);
							Jag4.Set((float) 0.0);
							Wait(0.005);				// wait for a motor update time
						if(SmartDashboard::GetBoolean ("Start Jag Test"))
						{
							JagCalTimer.Reset();
							JagCalTimer.Start();
							float myTime1=SmartDashboard::GetNumber("Zero Time");
								float myTime2=SmartDashboard::GetNumber("Run Time");
								mySpeed=SmartDashboard::GetNumber("Jag Calibration Run Speed");
							while(!JagCalTimer.HasPeriodPassed(myTime1))
							{
								SmartDashboard::PutNumber("Timer1",JagCalTimer.Get());
								Jag1.Set((float) 0.0);
								Jag2.Set((float)0.0);
								Jag3.Set((float)0.0);
								Jag4.Set((float)0.0);
								Jag1.Calibrate();
								Jag2.Calibrate();
								Jag3.Calibrate();
								Jag4.Calibrate();
								Wait(0.005);				// wait for a motor update time
							}
							JagCalTimer.Reset();
							JagCalTimer.Start();
							while(!JagCalTimer.HasPeriodPassed(myTime2)) 
								{
								SmartDashboard::PutNumber("Timer2",JagCalTimer.Get());
								Jag1.Set(-mySpeed);
								Jag2.Set(-mySpeed);
								Jag3.Set(mySpeed);
								Jag4.Set(mySpeed);
								Jag1.Calibrate();
								Jag2.Calibrate();
								Jag3.Calibrate();
								Jag4.Calibrate();
								Wait(0.005);				// wait for a motor update time

								}
							SmartDashboard::PutBoolean ("Start Jag Test",false);
							Jag1.Set((float) 0.0);
							Jag2.Set((float)0.0);
							Jag3.Set((float)0.0);
							Jag4.Set((float)0.0);
							Wait(0.005);				// wait for a motor update time
							}
						else
							{
							Jag1.Set((float) 0.0);
							Jag2.Set((float)0.0);
							Jag3.Set((float)0.0);
							Jag4.Set((float)0.0);
							Wait(0.005);				// wait for a motor update time
							}
						}
#endif
					}
					else
					{
						if(!BlinkFlg){BlinkFlg=true;CalTimer.Reset(),CalTimer.Start();}
					}
					if(BlinkFlg)
					{
						if(Blinker)
						{
							SmartDashboard::PutBoolean("Calibrate",true);
						}
						else
						{
							SmartDashboard::PutBoolean("Calibrate",false);

						}
						if(CalTimer.HasPeriodPassed(CAL_BLINK))Blinker=!Blinker;
					}
					
			Wait(.005);
			
			}
		SmartDashboard::PutBoolean("Calibrate",false);
		CalTimer.Stop();
		*/
	}
/*void Log()
{
	if(!IsLogInitialized)
	{
		IsLogInitialized=true;
		SmartDashboard::PutBoolean("Log Jag1 Check",true);
		SmartDashboard::PutBoolean("Log Jag2 Check",true);
		SmartDashboard::PutBoolean("Log Jag3 Check",true);
		SmartDashboard::PutBoolean("Log Jag4 Check",true);
	}
	if(SmartDashboard::GetBoolean("Log Jag1 Check"))
	{
		SmartDashboard::PutNumber("Control Mode 1",Jag1.GetControlMode());
		SmartDashboard::PutNumber("Faults 1",Jag1.GetFaults());
		SmartDashboard::PutNumber("Set Value 1",Jag1.Get());
		SmartDashboard::PutNumber("Current 1",Jag1.GetOutputCurrent());
	}
	if(SmartDashboard::GetBoolean("Log Jag2 Check"))
	{
		SmartDashboard::PutNumber("Control Mode 1",Jag1.GetControlMode());
		SmartDashboard::PutNumber("Faults 2",Jag2.GetFaults());
		SmartDashboard::PutNumber("Set Value 2",Jag2.Get());
		SmartDashboard::PutNumber("Current 2",Jag2.GetOutputCurrent());
	}
	if(SmartDashboard::GetBoolean("Log Jag3 Check"))
	{
		SmartDashboard::PutNumber("Control Mode 3",Jag3.GetControlMode());
		SmartDashboard::PutNumber("Faults 3",Jag3.GetFaults());
		SmartDashboard::PutNumber("Set Value 3",Jag3.Get());
		SmartDashboard::PutNumber("Current 3",Jag3.GetOutputCurrent());
	}
	if(SmartDashboard::GetBoolean("Log Jag4 Check"))
	{
		SmartDashboard::PutNumber("Control Mode 4",Jag4.GetControlMode());
		SmartDashboard::PutNumber("Faults 4",Jag4.GetFaults());
		SmartDashboard::PutNumber("Set Value 4",Jag4.Get());
		SmartDashboard::PutNumber("Current 4",Jag4.GetOutputCurrent());
	}
}*/
};

START_ROBOT_CLASS(RobotDemo);

